#include "CQuadrotor.h"

CQuadrotor::CQuadrotor()
{

        model.init();
}

CQuadrotor::~CQuadrotor()
{

}


void CQuadrotor::init()
{
 model.init();
}

CPose3D CQuadrotor::getRenderPose()
{
 return model.getRenderPose();
}

void CQuadrotor::simulate()
{
      vector_double pose_noise(3);
      vector_double v_noise(3);

	mrpt::random::vectorRandomNormal(v_noise, 0.0,0.3);
	mrpt::random::vectorRandomNormal(pose_noise, 0.0,0.1);
        //pilot.piloteWpt(model.m_pose,TPoint3D(model.getVx(),model.getVy(),0));
        CPose3D tt(model.m_pose);
        tt.x(tt.x()+pose_noise[0]);
        tt.y(tt.y()+pose_noise[1]);
        tt.z(tt.z()+pose_noise[2]);
        
        pilot.estimation(laser.scan_obs,model.m_pose);
        
       pilot.piloteHuit(laser.scan_obs,tt,TPoint3D(model.getVx()+v_noise[0],model.getVy()+v_noise[1],0));
     //  pilot.piloteWpt(model.m_pose,TPoint3D(model.getVx(),model.getVy(),0));
        model.SetCommands(pilot.getPhiC(),pilot.getThetaC(),pilot.getPsiC(),-2);
        model.simulate();
        laser.setPose(model.getRenderPose());
        laser.simulateLaserScan(view);

}

void CQuadrotor::setView(COpenGLViewportPtr view_ )
{
      view = view_;
}
